#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include <termios.h>
#include <unistd.h>
#include "basic_dev.hpp"

using namespace std;

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "keyboard_control");
    ros::NodeHandle nh;
    ros::Publisher pub_keyboard = nh.advertise<std_msgs::Int32>("/switch", 10);
    std_msgs::Int32 key;
    //BasicDev go(&nh);
    double angular_z, linear_x, linear_y, linear_z;

    struct termios new_settings;
    struct termios stored_settings;
    tcgetattr(0, &stored_settings);
    new_settings = stored_settings;
    new_settings.c_lflag &= (~ICANON);
    new_settings.c_cc[VTIME] = 0;
    new_settings.c_cc[VMIN] = 1;
    tcsetattr(0, TCSANOW, &new_settings);

    while (ros::ok())
    {
        int in = getchar();
        if (in == 'q') {
            break; // 按'q'退出
        }
        // 根据按键设置消息内容
        if (in == 'w') {
            key.data = 1; // 前进
            angular_z = 0;
            linear_x = 1;
            linear_y = 0;
            linear_z = 0;

        } else if (in == 's') {
            key.data = 2; // 后退
            angular_z = 0;
            linear_x = -1;
            linear_y = 0;
            linear_z = 0;;

        } else if (in == 'a') {
            key.data = 3; // 左移
        } else if (in == 'd') {
            key.data = 4; // 右移
        } else {
            key.data = 0; // 其他
        }
        ROS_INFO("\n Key pressed: %c, Sending: %d", in, key.data);
        pub_keyboard.publish(key);
        //go.set_vel(angular_z, linear_x, linear_y, linear_z);
        ros::spinOnce();
    }

    // 恢复终端设置
    tcsetattr(0, TCSANOW, &stored_settings);
    return 0;
}